import time
import math
import smbus
import logging


class ServoController:
    """舵机控制器，用于控制PCA9685舵机驱动板"""

    def __init__(self, i2c_address=0x40, frequency=50, debug=False):
        """
        初始化舵机控制器

        参数:
            i2c_address: PCA9685的I2C地址，默认0x40
            frequency: PWM频率，默认50Hz(适合大多数舵机)
            debug: 是否启用调试模式
        """
        self.bus = smbus.SMBus(1)
        self.address = i2c_address
        self.debug = debug

        # 配置日志
        self.logger = logging.getLogger('ServoController')
        if debug:
            self.logger.setLevel(logging.DEBUG)
        else:
            self.logger.setLevel(logging.INFO)
        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        ch = logging.StreamHandler()
        ch.setFormatter(formatter)
        self.logger.addHandler(ch)

        # PCA9685寄存器地址
        self.__SUBADR1 = 0x02
        self.__SUBADR2 = 0x03
        self.__SUBADR3 = 0x04
        self.__MODE1 = 0x00
        self.__PRESCALE = 0xFE
        self.__LED0_ON_L = 0x06
        self.__LED0_ON_H = 0x07
        self.__LED0_OFF_L = 0x08
        self.__LED0_OFF_H = 0x09
        self.__ALLLED_ON_L = 0xFA
        self.__ALLLED_ON_H = 0xFB
        self.__ALLLED_OFF_L = 0xFC
        self.__ALLLED_OFF_H = 0xFD

        self.logger.info(f"初始化PCA9685，地址: 0x{self.address:02X}, 频率: {frequency}Hz")
        self.write(self.__MODE1, 0x00)
        self.setPWMFreq(frequency)

    def write(self, reg, value):
        """向指定寄存器写入值"""
        try:
            self.bus.write_byte_data(self.address, reg, value)
            if self.debug:
                self.logger.debug(f"I2C: 写入0x{value:02X}到寄存器0x{reg:02X}")
        except Exception as e:
            self.logger.error(f"写入寄存器失败 (0x{reg:02X}): {str(e)}")
            raise

    def read(self, reg):
        """从指定寄存器读取值"""
        try:
            result = self.bus.read_byte_data(self.address, reg)
            if self.debug:
                self.logger.debug(f"I2C: 从寄存器0x{reg:02X}读取到0x{result:02X}")
            return result
        except Exception as e:
            self.logger.error(f"读取寄存器失败 (0x{reg:02X}): {str(e)}")
            raise

    def setPWMFreq(self, freq):
        """设置PWM频率"""
        prescaleval = 25000000.0  # 25MHz
        prescaleval /= 4096.0  # 12位分辨率
        prescaleval /= float(freq)
        prescaleval -= 1.0

        self.logger.debug(f"设置PWM频率为 {freq}Hz")
        self.logger.debug(f"估算的预分频值: {prescaleval}")

        prescale = math.floor(prescaleval + 0.5)
        self.logger.debug(f"最终预分频值: {prescale}")

        oldmode = self.read(self.__MODE1)
        newmode = (oldmode & 0x7F) | 0x10  # 进入睡眠模式
        self.write(self.__MODE1, newmode)  # 进入睡眠状态
        self.write(self.__PRESCALE, int(math.floor(prescale)))
        self.write(self.__MODE1, oldmode)
        time.sleep(0.005)
        self.write(self.__MODE1, oldmode | 0x80)  # 启用自动重装载

    def setPWM(self, channel, on, off):
        """设置单个PWM通道"""
        if channel < 0 or channel > 15:
            raise ValueError(f"通道号必须在0-15之间，当前值: {channel}")

        self.write(self.__LED0_ON_L + 4 * channel, on & 0xFF)
        self.write(self.__LED0_ON_H + 4 * channel, on >> 8)
        self.write(self.__LED0_OFF_L + 4 * channel, off & 0xFF)
        self.write(self.__LED0_OFF_H + 4 * channel, off >> 8)

        if self.debug:
            self.logger.debug(f"通道: {channel}, LED_ON: {on}, LED_OFF: {off}")

    def setDutycycle(self, channel, pulse):
        """设置占空比(0-100)"""
        if pulse < 0 or pulse > 100:
            raise ValueError(f"占空比必须在0-100%之间，当前值: {pulse}%")
        self.setPWM(channel, 0, int(pulse * (4096 / 100)))


class TwoAxisPanTilt:
    """二维云台控制器"""

    def __init__(self,
                 horizontal_channel=9,
                 vertical_channel=10,
                 i2c_address=0x40,
                 pwm_frequency=50,
                 horizontal_min_angle=0,
                 horizontal_max_angle=180,
                 vertical_min_angle=0,
                 vertical_max_angle=180,
                 horizontal_offset=0,
                 vertical_offset=0,
                 movement_delay=0.3,
                 debug=False):
        """
        初始化二维云台控制器

        参数:
            horizontal_channel: 水平舵机通道号
            vertical_channel: 垂直舵机通道号
            i2c_address: PCA9685的I2C地址
            pwm_frequency: PWM频率
            horizontal_min_angle: 水平最小角度
            horizontal_max_angle: 水平最大角度
            vertical_min_angle: 垂直最小角度
            vertical_max_angle: 垂直最大角度
            horizontal_offset: 水平角度偏移量
            vertical_offset: 垂直角度偏移量
            movement_delay: 舵机移动延迟(秒)
            debug: 是否启用调试模式
        """
        self.horizontal_channel = horizontal_channel
        self.vertical_channel = vertical_channel
        self.horizontal_min_angle = horizontal_min_angle
        self.horizontal_max_angle = horizontal_max_angle
        self.vertical_min_angle = vertical_min_angle
        self.vertical_max_angle = vertical_max_angle
        self.horizontal_offset = horizontal_offset
        self.vertical_offset = vertical_offset
        self.movement_delay = movement_delay

        # 初始化PCA9685控制器
        self.controller = ServoController(i2c_address, pwm_frequency, debug)

        # 当前角度
        self.current_horizontal_angle = None
        self.current_vertical_angle = None

        # 舵机参数配置
        # 角度到脉冲宽度的映射参数(默认值适合大多数舵机)
        self.pulse_min = 102  # 对应2.5%占空比 (4096 * 0.025)
        self.pulse_max = 512  # 对应12.5%占空比 (4096 * 0.125)

        self.logger = logging.getLogger('TwoAxisPanTilt')
        if debug:
            self.logger.setLevel(logging.DEBUG)
        else:
            self.logger.setLevel(logging.INFO)
        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        ch = logging.StreamHandler()
        ch.setFormatter(formatter)
        self.logger.addHandler(ch)

        self.logger.info(f"二维云台控制器初始化完成")
        self.logger.info(
            f"水平舵机: 通道={horizontal_channel}, 角度范围={horizontal_min_angle}-{horizontal_max_angle}度")
        self.logger.info(f"垂直舵机: 通道={vertical_channel}, 角度范围={vertical_min_angle}-{vertical_max_angle}度")

    def _angle_to_pulse(self, angle, min_angle, max_angle):
        """将角度转换为PWM脉冲值"""
        # 限制角度范围
        angle = max(min_angle, min(max_angle, angle))

        # 线性映射角度到脉冲宽度
        pulse = self.pulse_min + (angle - min_angle) * (self.pulse_max - self.pulse_min) / (max_angle - min_angle)
        return int(round(pulse))

    def set_horizontal_angle(self, angle):
        """设置水平舵机角度"""
        # 应用偏移量
        adjusted_angle = angle + self.horizontal_offset

        # 检查角度范围
        if adjusted_angle < self.horizontal_min_angle or adjusted_angle > self.horizontal_max_angle:
            self.logger.warning(f"水平角度超出范围: {angle}度 (调整后: {adjusted_angle}度)")
            adjusted_angle = max(self.horizontal_min_angle, min(self.horizontal_max_angle, adjusted_angle))

        # 转换角度为脉冲宽度
        pulse = self._angle_to_pulse(adjusted_angle, self.horizontal_min_angle, self.horizontal_max_angle)

        # 设置PWM
        self.logger.info(f"设置水平角度: {angle}度 (调整后: {adjusted_angle}度)")
        self.controller.setPWM(self.horizontal_channel, 0, pulse)
        self.current_horizontal_angle = angle

        # 等待舵机移动
        time.sleep(self.movement_delay)

    def set_vertical_angle(self, angle):
        """设置垂直舵机角度"""
        # 应用偏移量
        adjusted_angle = angle + self.vertical_offset

        # 检查角度范围
        if adjusted_angle < self.vertical_min_angle or adjusted_angle > self.vertical_max_angle:
            self.logger.warning(f"垂直角度超出范围: {angle}度 (调整后: {adjusted_angle}度)")
            adjusted_angle = max(self.vertical_min_angle, min(self.vertical_max_angle, adjusted_angle))

        # 转换角度为脉冲宽度
        pulse = self._angle_to_pulse(adjusted_angle, self.vertical_min_angle, self.vertical_max_angle)

        # 设置PWM
        self.logger.info(f"设置垂直角度: {angle}度 (调整后: {adjusted_angle}度)")
        self.controller.setPWM(self.vertical_channel, 0, pulse)
        self.current_vertical_angle = angle

        # 等待舵机移动
        time.sleep(self.movement_delay)

    def set_angle(self, horizontal_angle, vertical_angle):
        """同时设置水平和垂直角度"""
        self.set_horizontal_angle(horizontal_angle)
        self.set_vertical_angle(vertical_angle)

    def move_horizontal(self, delta_angle):
        """水平方向移动指定角度"""
        if self.current_horizontal_angle is None:
            self.set_horizontal_angle(0)

        new_angle = self.current_horizontal_angle + delta_angle
        self.set_horizontal_angle(new_angle)

    def move_vertical(self, delta_angle):
        """垂直方向移动指定角度"""
        if self.current_vertical_angle is None:
            self.set_vertical_angle(0)

        new_angle = self.current_vertical_angle + delta_angle
        self.set_vertical_angle(new_angle)

    def center(self):
        """将云台移动到中心位置"""
        center_horizontal = (self.horizontal_min_angle + self.horizontal_max_angle) / 2
        center_vertical = (self.vertical_min_angle + self.vertical_max_angle) / 2
        self.set_angle(center_horizontal, center_vertical)


# 云台配置参数
PAN_TILT_CONFIG = {
    # 舵机通道配置
    'horizontal_channel': 9,  # 水平舵机通道
    'vertical_channel': 10,  # 垂直舵机通道

    # PCA9685配置
    'i2c_address': 0x40,  # PCA9685 I2C地址
    'pwm_frequency': 50,  # PWM频率(Hz)

    # 角度范围配置
    'horizontal_min_angle': 0,  # 水平最小角度
    'horizontal_max_angle': 180,  # 水平最大角度
    'vertical_min_angle': 0,  # 垂直最小角度
    'vertical_max_angle': 180,  # 垂直最大角度

    # 角度偏移配置(用于校准)
    'horizontal_offset': 0,  # 水平角度偏移
    'vertical_offset': 0,  # 垂直角度偏移

    # 移动参数
    'movement_delay': 0.3,  # 每次移动后的延迟时间(秒)

    # 调试模式
    'debug': False  # 是否启用调试模式
}


def main():
    """示例：控制二维云台转动"""
    try:
        # 创建云台控制器实例
        pan_tilt = TwoAxisPanTilt(**PAN_TILT_CONFIG)

        # 移动到中心位置
        pan_tilt.center()
        print("云台已移动到中心位置")
        time.sleep(1)

        # 水平方向扫描
        #print("开始水平扫描...")
        #for angle in [90,60,90]:
            #pan_tilt.set_horizontal_angle(angle)
            #print(f"水平角度: {angle}度")
            #time.sleep(1)

        # 垂直方向扫描
        print("开始垂直扫描...")
        for angle in [90,60,90]:
            pan_tilt.set_vertical_angle(angle)
            print(f"垂直角度: {angle}度")
            time.sleep(1)

        # 组合移动
        #print("开始组合移动...")
        #pan_tilt.set_angle(45, 45)
        #print("位置: 水平45度, 垂直45度")
        #time.sleep(1)

        #pan_tilt.set_angle(135, 135)
        #print("位置: 水平135度, 垂直135度")
        #time.sleep(1)

        # 回到中心位置
        #pan_tilt.center()
        #print("云台已回到中心位置")

    except KeyboardInterrupt:
        print("程序被用户中断")
    except Exception as e:
        print(f"发生错误: {str(e)}")
    finally:
        print("程序已结束")


if __name__ == "__main__":
    main()
